#include "ros/ros.h"
#include "auto_msgs/localizition.h"
#include "auto_msgs/chassis.h"
#include "auto_msgs/control_cmd.h"
#include "auto_msgs/planning.h"
#include "auto_msgs/planning_point.h"
#include "common_tool/clampPid.h"
#include "common_tool/watchDog.h"
#include <memory>
#include "data_2d2s_path.h"
#include <yaml-cpp/yaml.h>
auto_msgs::chassis chassis_msg;
auto_msgs::control_cmd control_cmd_msg;
double rate = 100;
/*    watchDog(int foodPerTurn=0, int hungerPerTurn=0, int hungerMin=0,\
    int criticalPoint=50, int hungerMax=100); */
watchDog chassis_dog(8, 1, 0, 3, 8);
/*    clampPid( double Kp_local,double Ki_local,double Kd_local,\
              double deltaTime_local,\
              double downLimit_local,\
              double upLimit_local,\
              double IntergrationDownLimit_local,\
              double  IntergrationUplimit_local );*/
clampPid steer_angle_pid(1.0, 50, 0.0, 1.0 / rate, -10, 10, -10, 10);

void chassis_cb(const auto_msgs::chassis &msg)
{
	chassis_msg = msg;
	chassis_dog.giveFood();
	std::cout << "chassis msg is received:" << std::endl;
	std::cout << "tire f angle is:" << chassis_msg.steering_angle_f << std::endl;
}
int main(int argc, char **argv)
{
	std::string file_name = data_2d2s_path + "steer_test.yaml";
	YAML::Node steer_test_ymal = YAML::LoadFile(file_name); //也可以这样读取文件
	double angle_cmd = steer_test_ymal["angle_cmd"].as<double>();
	ros::init(argc, argv, "talker"); //初始化ros，向master注册一个叫“talker”的节点
	ros::NodeHandle n;				 //初始化一个句柄，就是将ROS实例化
	ros::Publisher control_cmd_pub = n.advertise<auto_msgs::control_cmd>("control_cmd", 3);
	ros::Subscriber chassis_sub_ =
		n.subscribe("chassis", 3, chassis_cb);
	//消息队列相当于缓存消息，如果处理速度不够快的话消息会被保存在队列中
	ros::Rate loop_rate(rate); //执行循环的速率，可以理解为发布消息的频率，单位是Hz

	int count = 0;
	while (ros::ok()) //ros::ok()函数用来判断master节点是否正常
	{
		ros::spinOnce();
		/*if (chassis_dog.isDied())
		{
			std::cout << "chassis msg timeout" << std::endl;
			steer_angle_pid.resetIntergration();
			continue;
		}*/
		double angle_error_degree = (angle_cmd - chassis_msg.steering_angle_f) * 3.14 / 180;
		std::cout << "###angle_cmd  is:" << angle_cmd << std::endl;
		std::cout << "###feed_back tire angle is:" << chassis_msg.steering_angle_f << std::endl;
		std::cout << "angle error is:" << angle_error_degree << std::endl;
		double steer_torque = steer_angle_pid.step(angle_error_degree);
		std::cout << "steering torque is:" << steer_torque << std::endl;
		control_cmd_msg.tire_angle_target_f = steer_torque;
		control_cmd_msg.throttle = 0;
		control_cmd_msg.brake = 0.0;
		control_cmd_pub.publish(control_cmd_msg); //发布该消息
												  //动作执行一次
		chassis_dog.noFood();
		loop_rate.sleep();
	}
	return 0;
}